﻿#region using statements

using System;
using System.Collections.Generic;
using System.Threading;

using Microsoft.Xna.Framework;

using NGen.Collections;

#endregion

namespace NGen.Managers.Pathfinding
{
	/// <summary>
	/// 	Implmentation of a 2d pathfinding system using A* algorithm
	/// </summary>
	public class ManagerPathfinding : IManagerPathfinding
	{
		#region local fields

		/// <summary>
		/// 	Stores the possible location of child cells
		/// </summary>
		private readonly bool[,] m_array2DChildLocation = new bool[3,3];

		/// <summary>
		/// 	The open list of cells
		/// </summary>
		private readonly Heap<PathfindingCell> m_hOpenCells = new Heap<PathfindingCell>();

		/// <summary>
		/// 	The cells which have already been visited
		/// </summary>
		private readonly HashSet<PathfindingCell> m_hsclosedCells = new HashSet<PathfindingCell>();

		/// <summary>
		/// 	Array containing all of the cell objects
		/// </summary>
		private PathfindingCell[,] m_array2DCells;

		/// <summary>
		/// 	The current cell being evaluted
		/// </summary>
		private PathfindingCell m_currentCell;

		/// <summary>
		/// 	The current call location in the array being evaluated
		/// </summary>
		private Point m_currentLocation;

		/// <summary>
		/// 	have the local variables been initialized
		/// </summary>
		private bool m_isInitialized;

		/// <summary>
		/// 	The temporary cell value used to process the child cells
		/// </summary>
		private PathfindingCell m_tempCell;

		/// <summary>
		/// 	The temporary cell location used to process child cells
		/// </summary>
		private Point m_tempLocation;

		#endregion

		#region local methods

		/// <summary>
		/// 	_Initialize the pathfinding manager. This should be called before
		/// 	each pathfinding search
		/// </summary>
		public void Prepare() {
			//remove everything from the open list
			m_hOpenCells.Clear();

			m_currentLocation.X = 0;
			m_currentLocation.Y = 0;

			//initialize the cell map
			for(var x = 0; x < NumColumns; x++) {
				for(var y = 0; y < NumRows; y++) {
					m_array2DCells[x, y].m_gCost = 0x7fffffff;
					m_array2DCells[x, y].m_previousCell = m_currentLocation;
				}
			}

			m_hsclosedCells.Clear();
		}

		/// <summary>
		/// 	Returns the heuristic cost between the two points. Works with or
		/// 	without diagonal movement
		/// </summary>
		/// <param name = "p_p1">position 1 to evaluate</param>
		/// <param name = "p_p2">position 2 to evaluate</param>
		/// <returns> heuristic cost between the two points passed in</returns>
		private double _GetHCost(Point p_p1, Point p_p2) {
			//return manhattan distance if diagonal movement is not allowed
			if(DiagonalMoveCost == 0.0f) {
				return AdjacentMoveCost * (Math.Abs(p_p1.X - p_p2.X) + Math.Abs(p_p1.Y - p_p2.Y));
			}

			//else return diagonal distance
			double t1 = Math.Min(Math.Abs(p_p1.X - p_p2.X), Math.Abs(p_p1.Y - p_p2.Y));
			return (DiagonalMoveCost * t1) + (AdjacentMoveCost * ((Math.Abs(p_p1.X - p_p2.X) + Math.Abs(p_p1.Y - p_p2.Y)) - 2 * t1));
		}

		#endregion

		#region IManagerPathfinding implementation

		/// <summary>
		/// 	<para>Get or set the number of columns</para>
		/// </summary>
		public int NumColumns { get; set; }

		/// <summary>
		/// 	<para>Get or set the number of rows</para>
		/// </summary>
		public int NumRows { get; set; }

		/// <summary>
		/// 	Get or set the cost to move to adjacent cells. Non-positive
		/// 	values will cause incorrect paths.
		/// </summary>
		public double AdjacentMoveCost { get; set; }

		/// <summary>
		/// 	Get or set the cost to move to diagonal squares.
		/// 	(0 to disable diagonal movement)
		/// </summary>
		public double DiagonalMoveCost { get; set; }

		/// <summary>
		/// 	<para>Get or set the 2d Array containing the map layout, used to
		/// 		determine if a cell is passable.</para>
		/// 	<para>true if passable, false if not</para>
		/// </summary>
		public bool[,] GridLayout { get; set; }

		/// <summary>
		///		<para>get or set the cost multiplier of passing through each specific cell.</para>
		///		<para>default values for each entry is 1</para>
		/// </summary>
		public float[,] CellCostsMultiplier { get; set; }

		/// <summary>
		/// 	Should this manager maintain a closed list. Better performance when
		/// 	this value is false
		/// </summary>
		public bool MaintainClosedList { get; set; }

		/// <summary>
		/// 	Gets an array containing the cells which were visited
		/// </summary>
		public PathfindingCell[] ClosedList {
			get {
				if(!MaintainClosedList) {
					return null;
				}

				var tempList = new PathfindingCell[m_hsclosedCells.Count];
				m_hsclosedCells.CopyTo(tempList);
				return tempList;
			}
		}

		/// <summary>
		/// 	Gets an array containing the cells which were seen, but not visited
		/// </summary>
		public PathfindingCell[] OpenList {
			get {
				var returnVal = new PathfindingCell[m_hOpenCells.GetContents.Length];
				Array.Copy(m_hOpenCells.GetContents, returnVal, m_hOpenCells.Count);
				return returnVal;
			}
		}

		/// <summary>
		/// 	Set pathfinding parameters
		/// </summary>
		/// <param name = "p_numColumns">number of rows</param>
		/// <param name = "p_numRows">number of columns</param>
		/// <param name = "p_adjMoveCost">adjacent move cost, must be positive</param>
		/// <param name = "p_diagMoveCost">diagonal move cost. must be positive, or
		/// 	0 to disable diagonal movement, or negative to have it 
		/// 	calculated to be in proportion to the adjacent move cost</param>
		/// <param name = "p_passableCellMap">array containing the true/false values to 
		/// 	determine if a cell is passable. true for passable, false for 
		/// 	impassable</param>
		public void SetParams(int p_numColumns, int p_numRows, double p_adjMoveCost, double p_diagMoveCost, bool[,] p_passableCellMap) {
			//minimum size of 10 by 10 grid
			NumColumns = p_numColumns >= 10 ? p_numColumns : 10;
			NumRows = p_numRows >= 10 ? p_numRows : 10;

			//ensure minimum value of 1
			AdjacentMoveCost = p_adjMoveCost > 0 ? p_adjMoveCost : 1;

			//calculate the diagonal movement cost
			DiagonalMoveCost = p_diagMoveCost >= 0 ? p_diagMoveCost : AdjacentMoveCost * 1.4142;

			m_array2DCells = new PathfindingCell[NumColumns,NumRows];

			//if the map array is the correct size then copy it
			if(p_passableCellMap.GetUpperBound(0) == NumColumns - 1 &&
			   p_passableCellMap.GetUpperBound(1) == NumRows - 1) {
				GridLayout = p_passableCellMap;
				CellCostsMultiplier = new float[NumColumns, NumRows];
				for(var x=0; x<NumColumns; x++) {
					for(var y = 0; y < NumRows; y++) {
						CellCostsMultiplier[x,y] = 1f;
					}
				}
				m_isInitialized = true;
				return;
			}

			//else throw an exception
			throw new Exception("passableCellMap array passed in was not of the " + "specified dimensions");
		}

		/// <summary>
		/// 	Returns an array of Points which contain the optimal path
		/// 	from the start to the finish. An array of 0 elements is returned
		/// 	if no path can be found. This is an implementation of the A*
		/// 	algorithm
		/// </summary>
		/// <param name = "p_startLocation">location to start searching</param>
		/// <param name = "p_endLocation">location to end searching</param>
		/// <returns>the array containing the coordinates of the optimal path
		/// </returns>
		public Point[] FindPath(Point p_startLocation, Point p_endLocation) {
			if(!m_isInitialized) {
				throw new Exception("search values have not been set. you must first call " + "the SetPrams() method");
			}

			if(p_startLocation.Equals(p_endLocation)) {
				return null;
			}

			//set local values to start a new search
			Prepare();

			m_array2DCells[p_startLocation.X, p_startLocation.Y].m_gCost = 0;
			m_array2DCells[p_startLocation.X, p_startLocation.Y].m_hCost = _GetHCost(p_startLocation, p_endLocation);
			m_array2DCells[p_startLocation.X, p_startLocation.Y].m_previousCell = p_startLocation;
			m_array2DCells[p_startLocation.X, p_startLocation.Y].m_location = p_startLocation;
			m_currentCell = m_array2DCells[p_startLocation.X, p_startLocation.Y];
			m_currentLocation = p_startLocation;

			//for loop variables, cache optimizations
			int x;

			//loop until the ending location is found
			while(m_currentLocation != p_endLocation) {
				int y;
				for(x = 0; x < 3; ++x) {
					for(y = 0; y < 3; ++y) {
						m_array2DChildLocation[x, y] = true;
					}
				}

				//current location is not a valid move
				m_array2DChildLocation[1, 1] = false;

				//updates the array to make sure it only contains valid child cells
				if(m_currentLocation.X == 0) {
					m_array2DChildLocation[0, 0] = m_array2DChildLocation[0, 1] = m_array2DChildLocation[0, 2] = false;
				} else if(m_currentLocation.X == NumColumns) {
					m_array2DChildLocation[2, 0] = m_array2DChildLocation[2, 1] = m_array2DChildLocation[2, 2] = false;
				}
				if(m_currentLocation.Y == 0) {
					m_array2DChildLocation[0, 0] = m_array2DChildLocation[1, 0] = m_array2DChildLocation[2, 0] = false;
				} else if(m_currentLocation.Y == NumColumns) {
					m_array2DChildLocation[0, 2] = m_array2DChildLocation[1, 2] = m_array2DChildLocation[2, 2] = false;
				}

				//remove the diagonal child cells if no diagonal movements allowed
				if(DiagonalMoveCost == 0) {
					m_array2DChildLocation[0, 0] = m_array2DChildLocation[2, 0] = m_array2DChildLocation[0, 2] = m_array2DChildLocation[2, 2] = false;
				}

				//iterate through each of the valid child cell moves
				for(x = -1; x < 2; ++x) {
					for(y = -1; y < 2; ++y) {
						m_tempLocation.X = m_currentLocation.X + x;
						m_tempLocation.Y = m_currentLocation.Y + y;

						//if not a valid child then skip this one
						if(!m_array2DChildLocation[x + 1, y + 1] || m_currentLocation.Y + y >= NumRows ||
						   m_currentLocation.X + x >= NumColumns) {
							continue;
						}

						//skip if this is a wall
						if(GridLayout[m_tempLocation.X, m_tempLocation.Y]) {
							continue;
						}

						m_tempCell = m_array2DCells[m_tempLocation.X, m_tempLocation.Y];

						//if the childs gCost is already less, then skip it
						//because there is already a shorter path to it
						if(m_tempCell.m_gCost <=
						   m_currentCell.m_gCost + ((Math.Abs(x - y) == 1 ? AdjacentMoveCost : DiagonalMoveCost) *
							 CellCostsMultiplier[m_currentCell.m_location.X, m_currentCell.m_location.Y])) {
							continue;
						}

						if(m_currentCell.m_previousCell.Equals(m_tempCell.m_location))
							continue;

						//set the gCost
						var gCost = (Math.Abs(x - y) == 1 ? AdjacentMoveCost : DiagonalMoveCost);
						gCost *= CellCostsMultiplier[m_currentCell.m_location.X, m_currentCell.m_location.Y];
						m_tempCell.m_gCost = gCost + m_currentCell.m_gCost;

						//set the hCost
						m_tempCell.m_hCost = _GetHCost(m_tempLocation, p_endLocation);
						m_tempCell.m_location.X = m_tempLocation.X;
						m_tempCell.m_location.Y = m_tempLocation.Y;
						m_tempCell.m_previousCell.X = m_currentLocation.X;
						m_tempCell.m_previousCell.Y = m_currentLocation.Y;

						//put it back in the array and onto the open list
						m_array2DCells[m_tempLocation.X, m_tempLocation.Y] = m_tempCell;
						m_hOpenCells.Push(m_tempCell);
					}
				}

				//if nothing left on the open list then a path does not exist, return null
				if(m_hOpenCells.Count == 0) {
					return null;
				}

				//only do this if activated
				if(MaintainClosedList) {
					m_hsclosedCells.Add(m_currentCell);
				}

				//grab next item from the open list evaluate from that position
				m_currentCell = m_hOpenCells.Pop();
				m_currentLocation = m_currentCell.m_location;
			}

			//calculate the size of the array needed for the return result
			x = 0;
			m_tempLocation = m_currentCell.m_location;
			while(m_tempLocation != p_startLocation) {
				m_tempLocation = m_currentCell.m_previousCell;
				m_currentCell = m_array2DCells[m_tempLocation.X, m_tempLocation.Y];
				++x;
			}

			m_currentCell = m_array2DCells[p_endLocation.X, p_endLocation.Y];
			var pathList = new Point[x--];

			//build the array containing the path
			while(x >= 0) {
				pathList[x] = m_currentCell.m_location;
				m_currentCell = m_array2DCells[m_currentCell.m_previousCell.X, m_currentCell.m_previousCell.Y];
				--x;
			}

			//clear the open list if we aren't worried about
			//maintaining search data
			if(!MaintainClosedList) {
				m_hOpenCells.Clear();
			}

			//return the optimal path to the target
			return pathList;
		}

		#endregion
	}
}